import serial
import time
import numpy as np
import odrive
import os
import plan 
import vision
import cv2
_teaching_mode_flag = 0
serial_is_connected = 0
begin_time = 0
pneumatic_states = [0, 0, 0, 0, 0]
pneumatic_table = [['a', 'A'],['b', 'B'],['c', 'C'],['d','D'],['e', 'E']]
last_rotate_states = 'abcd'
try:
    ser = serial.Serial('/dev/ttyUSB0',115200)
except Exception as error:
    print("Serial Error")
    print(error)
else:
    serial_is_connected = 1
    print("Serial Connected")
def control_pneumatic_valve(command):
    global pneumatic_states
    global last_rotate_states
    for i in range(4):
        if last_rotate_states[i] == chr(ord('a') + i):
            pneumatic_states[i] = 0
        else:
            pneumatic_states[i] = 1
    if command == 'Le':
        pneumatic_states[0] = (pneumatic_states[0] + 1) % 2
    if command == 'Lc':
        pneumatic_states[1] = (pneumatic_states[1] + 1) % 2
    if command == 'Re':
        pneumatic_states[3] = (pneumatic_states[3] + 1) % 2
    if command == 'Rc':
        pneumatic_states[2] = (pneumatic_states[2] + 1) % 2
    if command == 'Rise':
        pneumatic_states[4] = 1
    if command == 'Drop':
        pneumatic_states[4] = 0
    string_to_send = '' 
    for i in range(5):
        string_to_send += pneumatic_table[i][pneumatic_states[i]]
    last_rotate_states = string_to_send[:4] 
    if serial_is_connected:
        ser.write(string_to_send.encode()) 
    print(string_to_send)


base_accel = 100
ex_accel = 100
odrive_is_connected = 0
odrv = None
odrv_angles_bias = [-2092, -2494]
odrv_angles = [0 ,0] 
def odrive_connect():
    global odrive_is_connected
    global odrv
    try:
        print('Odrive start connecting')
        odrv = odrive.find_any() 
    except Exception as error:
        print("Odrive Error")
        print(error) 
    else:
        odrv.axis0.encoder.config.use_index = True
        odrv.axis0.controller.config.vel_limit = 2 * 10000 
        odrv.axis0.trap_traj.config.vel_limit = 1 * 10000 
        odrv.axis0.trap_traj.config.accel_limit = base_accel * 10000
        odrv.axis0.trap_traj.config.decel_limit = base_accel * 10000
        odrv.axis0.encoder.config.use_index = True
        odrv.axis0.controller.config.vel_limit = 10 * 10000 
        odrv.axis0.trap_traj.config.vel_limit = 9 * 10000 
        #another motor
        odrv.axis1.encoder.config.use_index = True
        odrv.axis1.controller.config.vel_limit = 2 * 10000 
        odrv.axis1.trap_traj.config.vel_limit = 1 * 10000 
        odrv.axis1.trap_traj.config.accel_limit = base_accel * 10000
        odrv.axis1.trap_traj.config.decel_limit = base_accel * 10000
        odrv.axis1.encoder.config.use_index = True
        odrv.axis1.controller.config.vel_limit = 10 * 10000 
        odrv.axis1.trap_traj.config.vel_limit = 9 * 10000 
        #done
        odrive_is_connected = 1
        print("Odrive Connected")

odrive_connect()

def odrive_calibration(motorID):
    global ser
    global odrv
    if motorID == 0:
        ser.write('ABcd'.encode())
        odrv.axis0.requested_state = 3
        while odrv.axis0.current_state != 1:
            time.sleep(0.1)
        odrv.axis0.requested_state = 8
        odrv.axis0.controller.move_to_pos(odrv_angles_bias[0])
        time.sleep(1)
    else:
        ser.write('abCD'.encode())
        odrv.axis1.requested_state = 3
        while odrv.axis1.current_state != 1:
            time.sleep(0.1)
        odrv.axis1.requested_state = 8
        odrv.axis1.controller.move_to_pos(odrv_angles_bias[1])
        time.sleep(1)

def odrive_disable():
    odrv.axis0.requested_state = 1
    odrv.axis1.requested_state = 1
def odrive_enable():
    odrv.axis0.requested_state = 8
    odrv.axis1.requested_state = 8

def check_based_position():
    if abs(odrv.axis0.encoder.pos_estimate - odrv_angles_bias[0]) >90 or abs(odrv.axis1.encoder.pos_estimate - odrv_angles_bias[1]) > 90: 
            print('Not start at based postion!')
            os.abort()

def odrive_rotate(motorID, angle):
    global odrv_angles_bias,odrv_angles
    delta_range = 30
    achieve_count = -5
    if angle == 180:
        odrv_angles[motorID] += 4096
    if angle == -180:
        odrv_angles[motorID] -= 4096
    if angle == 90:
        odrv_angles[motorID] += 2048
    if angle == 270:
        odrv_angles[motorID] -= 2048
    if angle == -90:
        odrv_angles[motorID] -= 2048
    pos = odrv_angles[motorID] + odrv_angles_bias[motorID]
    if motorID == 0:
        odrv.axis0.controller.move_to_pos(pos)
    else:
        odrv.axis1.controller.move_to_pos(pos)
    for i in range(500):
        if motorID == 0:
            pos_estimate = odrv.axis0.encoder.pos_estimate
        else:
            pos_estimate = odrv.axis1.encoder.pos_estimate
        delta = abs(pos_estimate - pos)
        if delta < delta_range:
            achieve_count += 1
        if achieve_count >=0:
            break
        if i > 100:
            odrv.axis0.requested_state = 1
            odrv.axis1.requested_state = 1
            print('Can not move to expect position, Be careful!')
            os.abort()
        #print('0:  %7.1f   1:  %7.1f'%(odrv_angles[0],odrv_angles[1]))
        #print('%4d  %4d  %5.2f   %5.2f'%(i,motorID,odrv_angles[motorID],delta))
        time.sleep(0.001)

    odrv.axis0.trap_traj.config.accel_limit = base_accel * 10000
    odrv.axis1.trap_traj.config.accel_limit = base_accel * 10000

# delay_time = 0.010
# clamp_time = 0.025 - delay_time 
print('请输入速度等级 （1-100）')
speed = int(input())
# speed = 1
delay_time = 0.030 * speed
clamp_time = speed * 0.015
release_time = 0.05 * speed
# delay_time = 0.030
# clamp_time = 0.045 - delay_time 
# release_time = 0.05
def cubic_rotate(command):
    global last_rotate_states
    dict = { #*1表示顺时针旋转90，*2表示旋转180，*3表示逆时针旋转90
            'L3': ['AbCD', ( 0 ,  90), 'ABcd', ( 0 , -90)],
            'R3': ['ABCd', ( 0 ,  90), 'ABcd', ( 0 , -90)],
            'U3': ['ABcD', ( 90,  0 ), 'abCD', (-90,  0 )],
            'D3': ['aBCD', ( 90 , 0 ), 'abCD', (-90,  0 )],
            'F3': ['abCD', ( 0 ,  90), 'ABcd', ( 0 , -90),
                   'aBCD', ( 90 , 0 ), 'abCD', (-90,  0 )],
            'B3': ['ABcd', ( 90 , 0 ), 'abCD', (-90,  0 ),
                   'ABCd', ( 0  , 90), 'ABcd', ( 0 , -90)],
            'L1': ['AbCD', ( 0 , -90), 'ABcd', ( 0 ,  90)],
            'R1': ['ABCd', ( 0 , -90), 'ABcd', ( 0 , +90)],
            'U1': ['ABcD', (-90,  0 ), 'abCD', (+90,  0 )],
            'D1': ['aBCD', (-90 , 0 ), 'abCD', (+90,  0 )],
            'F1': ['abCD', ( 0 ,  90), 'ABcd', ( 0 , -90),
                   'aBCD', (-90 , 0 ), 'abCD', (+90,  0 )],
            'B1': ['ABcd', ( 90 , 0 ), 'abCD', (-90,  0 ),
                   'ABCd', ( 0  ,-90), 'ABcd', ( 0 , +90)],
            'L2': ['AbCD', ( 0 , -180), 'AbCd' ,'ABcd', ( 0 , 180)],
            'R2': ['ABCd', ( 0 , -180), 'ABcd', ( 0 , 180)],
            'U2': ['ABcD', (180,  0  ), 'aBcD','abCD', (-180,  0 )],
            'D2': ['aBCD', (180 , 0  ), 'abCD', (-180,  0 )],
            'F2': ['abCD', ( 0 ,  90 ), 'ABcd', ( 0 , -90),
                   'aBCD', (180 , 0  ), 'abCD', (-180,  0 )],
            'B2': ['ABcd', ( 90 , 0 ), 'abCD', (-90,  0 ),
                   'ABCd', ( 0  ,-180), 'ABcd', ( 0 , 180)]


             }
    if odrive_is_connected:
        check_based_position()
    for item in dict[command]: 
        if isinstance(item, str):#判断是不是字符串（控制爪子的命令）
            if abs(ord(item[0]) - ord(item[1])) < 5:  #判断是不是翻面动作，如果是则限制电机加速度（两个控制字母的ASCLL码相邻表示只有一个手臂抓住了魔方，即将翻面）
                if abs(ord(item[2]) - ord(item[3])) < 5:
                    odrv.axis0.trap_traj.config.accel_limit = ex_accel * 10000
                    odrv.axis1.trap_traj.config.accel_limit = ex_accel * 10000
 
            print(last_rotate_states, item)
            string_to_send = ''
            if last_rotate_states == 'abcd':#如果是初始状态，则将last_置位成本次的相反控制，以达到先夹再松的效果以免掉落，如item=ABcd，last=abCD，则会先执行AB夹住
                last_rotate_states = ''
                for i in range(4):
                    if item[i] == chr(ord('a') + i):
                        last_rotate_states += chr(ord('A') + i)
                    else:
                        last_rotate_states += chr(ord('a') + i)
            for i in range(4):                          #先选出last是松开的(小写字母)这次是需要夹住的爪子夹住魔方
                if last_rotate_states[i] > item[i]:
                    string_to_send += item[i]
            if len(string_to_send):
                if serial_is_connected:
                    ser.write(string_to_send.encode())
                    print('ser:  '+string_to_send)
                else:
                    print(string_to_send)
                time.sleep(clamp_time)

            string_to_send = ''
            for i in range(4):                          #再选出last是夹住的(大写字母)这次是需要松开的爪子松开魔方
                if last_rotate_states[i] < item[i]:
                    string_to_send += item[i]
            if len(string_to_send):
                if serial_is_connected:
                    ser.write(string_to_send.encode())
                    print('ser:  '+string_to_send)
                else:
                    print(string_to_send)
                time.sleep(release_time)
            else:
                time.sleep(delay_time)

            last_rotate_states = item
        else:
            if odrive_is_connected and serial_is_connected:
                if item[0] != 0:
                    odrive_rotate(0 ,item[0])
                if item[1] != 0:
                    odrive_rotate(1 ,item[1])
            else:
                print(item)
                print('1111111')
                time.sleep(1)

#delay_time = 0.035
#clamp_time = 0.080 - delay_time 
#release_time = 0.080
def scan():
    time_scan = time.time()
    global last_rotate_states,pneumatic_states
    vision.update_surfaces()
    ser.write('AbcD'.encode())
    time.sleep(clamp_time)
    ser.write('e'.encode())
    time.sleep(0.1)
    ser.write('ABcD'.encode())
    time.sleep(clamp_time)
    ser.write('ABcd'.encode())
    time.sleep(release_time)
    odrive_rotate(0, -180)
    ser.write('ABcD'.encode())
    time.sleep(clamp_time)
    ser.write('aBcD'.encode())
    time.sleep(clamp_time)
    ser.write('aBCD'.encode())
    time.sleep(clamp_time)
    ser.write('abCD'.encode())
    time.sleep(release_time)
    vision.update_surfaces_B(1)
    odrive_rotate(0, 180)
    odrive_rotate(1, -180)
    ser.write('AbCD'.encode())
    time.sleep(clamp_time)
    ser.write('AbCd'.encode())
    time.sleep(clamp_time)
    ser.write('ABCd'.encode())
    time.sleep(clamp_time)
    ser.write('ABcd'.encode())
    time.sleep(release_time)
    vision.update_surfaces_A(3)
    odrive_rotate(1, 90)
    ser.write('ABCD'.encode())
    time.sleep(clamp_time)
    ser.write('abCD'.encode())
    time.sleep(release_time)
    odrive_rotate(1, -90)
    time.sleep(0.150)# can be lower
    vision.update_surfaces_A(2)
    odrive_rotate(1, 180)
    time.sleep(0.150)# can be lower
    vision.update_surfaces_A(5)
    last_rotate_states = 'abCD'
    pneumatic_states = [0, 0, 1, 1, 0]
    print('scan time: %5.3f s'%(time.time()-time_scan))

def scan_test():
    time_scan = time.time()
    global last_rotate_states,pneumatic_states
    vision.update_surfaces()
    ser.write('AbcD'.encode())
    time.sleep(clamp_time)
    ser.write('e'.encode())
    time.sleep(0.1)
    ser.write('ABcD'.encode())
    time.sleep(clamp_time)
    ser.write('ABcd'.encode())
    time.sleep(release_time)
    odrive_rotate(0, -180)
    ser.write('ABcD'.encode())
    time.sleep(clamp_time)
    ser.write('aBcD'.encode())
    time.sleep(clamp_time)
    ser.write('aBCD'.encode())
    time.sleep(clamp_time)
    ser.write('abCD'.encode())
    time.sleep(release_time)
    vision.update_surfaces_B(1)
    odrive_rotate(0, 180)
    odrive_rotate(1, -180)
    ser.write('AbCD'.encode())
    time.sleep(clamp_time)
    ser.write('AbCd'.encode())
    time.sleep(clamp_time)
    ser.write('ABCd'.encode())
    time.sleep(clamp_time)
    ser.write('ABcd'.encode())
    time.sleep(release_time)
    vision.update_surfaces_A(3)
    odrive_rotate(1, 90)
    ser.write('ABCD'.encode())
    time.sleep(clamp_time)
    ser.write('abCD'.encode())
    time.sleep(release_time)
    odrive_rotate(1, -90)
    time.sleep(0.150)# can be lower
    vision.update_surfaces_A(2)
    odrive_rotate(1, 180)
    time.sleep(0.150)# can be lower
    vision.update_surfaces_A(5)
    last_rotate_states = 'abCD'
    pneumatic_states = [0, 0, 1, 1, 0]
    print('scan time: %5.3f s'%(time.time()-time_scan))
    finished2()

def final():
    global begin_time,strat_status
    scan()
    # input()
    time.sleep(0.05)
    begin_time = time.time()
    strat_status = plan.surfaces_2_plan(vision.surfaces) 

    
def finished():
    global last_rotate_states,pneumatic_states 
    ser.write('ABCD'.encode())
    time.sleep(clamp_time)
    ser.write('AbcD'.encode())
    ser.write('E'.encode())
    time.sleep(0.12)
    ser.write('abcd'.encode())
    last_rotate_states = 'abcd'
    pneumatic_states = [0, 0, 0, 0, 0]
    cv2.destroyAllWindows()
    print('Total time: %5.3f s'%(time.time()-begin_time))
    print('是否放置完成？1/0\r\n')
    flag = int(input())
    if (flag == 1):
        scan()
        time.sleep(0.05)
        # plan.surfaces_3_plan('UUUUUUUUURRRRRRRRRFFFFFFFFFDDDDDDDDDLLLLLLLLLBBBBBBBBB', strat_status)
        plan.surfaces_3_plan(vision.surfaces, strat_status)

def finished1():
    global last_rotate_states,pneumatic_states, _teaching_mode_flag,clamp_time,delay_time,release_time
    ser.write('ABCD'.encode())
    time.sleep(clamp_time)
    ser.write('AbcD'.encode())
    ser.write('E'.encode())
    time.sleep(0.12)
    ser.write('abcd'.encode())
    last_rotate_states = 'abcd'
    pneumatic_states = [0, 0, 0, 0, 0]
    cv2.destroyAllWindows()
    print('是否开始教练模式？1/0\r\n')
    flag = int(input())
    if (flag == 1):
        scan()
        while True:
            time.sleep(0.05)
            hand_detect()
            flag_detect = get_paper(vision.Img_HandDetect)
            if (flag_detect == 1):
                break
        # time.sleep(3)
        delay_time = 0.030 * 50
        clamp_time = 50 * 0.015
        release_time = 0.05 * 50
        _teaching_mode_flag = 1
        time.sleep(0.05)
        plan.surfaces_4_plan(strat_status, 'UUUUUUUUURRRRRRRRRFFFFFFFFFDDDDDDDDDLLLLLLLLLBBBBBBBBB')

def finished2():
    global last_rotate_states,pneumatic_states,clamp_time,delay_time,release_time
    ser.write('ABCD'.encode())
    time.sleep(clamp_time)
    ser.write('AbcD'.encode())
    ser.write('E'.encode())
    time.sleep(0.12)
    ser.write('abcd'.encode())
    last_rotate_states = 'abcd'
    pneumatic_states = [0, 0, 0, 0, 0]
    cv2.destroyAllWindows()
    _teaching_mode_flag = 0
    delay_time = 0.030 * speed
    clamp_time = speed * 0.015
    release_time = 0.05 * speed


flag_find_paper =0
def get_paper(frame):
    global flag_find_paper  
    global inRange_hsv_g
    aero = 0
    gs_frame = cv2.GaussianBlur(frame, (5, 5), 0)                     # 高斯模糊
    hsv = cv2.cvtColor(gs_frame, cv2.COLOR_BGR2HSV)                 # 转化成HSV图像

    color_dist = {'white': {'Lower': np.array([0, 0, 34]), 'Upper': np.array([105, 16, 255])},
                }

    erode_hsv = cv2.erode(hsv,np.ones((11, 11), np.uint8), iterations=2)                   # 腐蚀 粗的变细
    inRange_hsv_g = cv2.inRange(erode_hsv, color_dist['white']['Lower'], color_dist['white']['Upper'])
    cnts_w = cv2.findContours(inRange_hsv_g.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[-2]    #找轮廓
    # cv2.imshow("inRange_hsv", inRange_hsv_g)
    cv2.imwrite('111.jpg',inRange_hsv_g)
    if len(cnts_w)!=0:
        c_w = max(cnts_w, key=cv2.contourArea)   #找最大面积轮廓
        rect_w = cv2.minAreaRect(c_w)     #最小外接矩形框
        box_w = cv2.boxPoints(rect_w)     #矩形框四点坐标
        cv2.drawContours(frame, [np.int0(box_w)], -1, (0, 255, 0), 2)     #画框
        aero = cv2.contourArea(c_w )   #得到轮廓面积
        # print(aero)
        if aero >8000 and flag_find_paper == 0:    #白色面积足够大（具体数值届时再算） 而且  上一状态是没有找到白纸  而且  现在魔方静止  
            print('find  paper     find  paper    find  paper   find  paper  ')
            print(aero)
            flag_find_paper = 1
            return 0
        if aero < 2000 and flag_find_paper == 1:   #白色面积小（具体数值届时再算） 而且  上一状态是已经找到白纸  而且  现在魔方静止  
            print('move    move    move   move move    move    move   move ')
            print(aero)
            flag_find_paper = 0
            return 1
        return 0
    elif flag_find_paper == 1:   #如果没有轮廓
        print('move    move    move   move move    move    move   move ')
        print('0')
        flag_find_paper = 0
        return 1
    return 0
    #flag_move = 1 的时候     机械运行    运行结束后 flag_ove 记得清0  才能继续下一循环

def hand_detect():
    vision.update_img()
    # cv2.imwrite('sssss.jpg',vision.Img_HandDetect)
    # cv2.imshow('detect',vision.Img_HandDetect)
    # cv2.waitKey(0)
# hand_detect()
count = 0
def send_command(command):
    global _teaching_mode_flag
    if command == 'Odrive':
        odrive_connect()
    if command == 'Cal0':
        odrive_calibration(0)
    if command == 'Cal1':
        odrive_calibration(1)

    if command == 'Scan':
        scan()
    if command == 'Scan_test':
        scan_test()
    if command == 'Final':
        final()

    if command == 'Finished':
        finished()

    if command == 'Finished1':
        finished1()

    if command == 'Finished2':
        finished2()
    if command in ['Le', 'Lc', 'Re', 'Rc', 'Rise', 'Drop']:
        control_pneumatic_valve(command)

    if odrive_is_connected:
        if command == 'OdEn':
            odrive_enable()
        if command == 'OdDis':
            odrive_disable()
        if command in ['Od0R1', 'Od0R3', 'Od0R2', 'Od1R1', 'Od1R3', 'Od1R2']:
            odrive_rotate(int(command[2]), int(command[-1]) * 90)
    if command in ['L1','L2','L3','R1','R2','R3','U1','U2','U3','D1','D2','D3','F1','F2','F3','B1','B2','B3']:
        if (_teaching_mode_flag == 1):
        
            while True:
                time.sleep(0.05)
                hand_detect()
                flag_detect = get_paper(vision.Img_HandDetect)
                if (flag_detect == 1):
                    break
                # cv2.imshow("inRange_hsv", inRange_hsv_g)
                #print(flag_find_paper)
        print('ssssssssssssssssss')
        cubic_rotate(command)

